/**
 * This class contains all code for controlling a robot particle
 * */
package com.mcp_31091.roomba.connection;

import static com.mcp_31091.roomba.RoombaActivity.wifiManager;
import java.util.HashMap;
import java.util.List;
import java.util.Random;
import android.net.wifi.ScanResult;
import android.util.Log;
import com.mcp_31091.roomba.WifiSignalMap.MapGenerator;
import com.mcp_31091.roomba.WifiSignalMap.MapPoint;

public class Robot {
//	private ConnectedThread controller;
	final static double MAP_LENGTH_MAX = 21000;
	final static double MAP_WIDTH_MAX = 5000;
	
	private int posX;
	private int posY;
	private double orientation;
	private double forward_noise;
	private double turn_noise;
	private double sense_noise;
	private double probabilty;
//	Context mcontext;
//	WifiManager wifiManager;
	
	
//	public Robot(Context context)
//	{
//		mcontext = context;
//		wifiManager = (WifiManager) mcontext.getSystemService(Context.WIFI_SERVICE);
//	}
	
	/*Generate a robot with a random x, y pos, orientation and sense_noise  */
	public Robot()
	{
		posX =  (int)(Math.random()*(MAP_LENGTH_MAX-1000) + 1000)/1000;
		posY =  (int)(Math.random()*(MAP_WIDTH_MAX-1000) + 1000)/1000;
		orientation =  Math.random()*2*Math.PI;
		probabilty = 1.0;
		sense_noise  = 5.0;
	}
	
	/*Generate a robot given its x, y and orientation*/
	public Robot(int x, int y, int heading)
	{	
		posX = x;
		posY = y;
		orientation = heading;
		probabilty = 1.0;
		sense_noise  = 5.0;
	}
	
	/*Generate a robot given its x, y, orientation and probability*/
	public Robot(int x, int y, int heading, double prob)
	{	
		posX = x;
		posY = y;
		orientation = heading;
		probabilty = 1.0;
		sense_noise  = 5.0;
		probabilty = prob;
	}
	
	/*Measure the probability of that point*/
	public double measureProb()
	{
		HashMap<String, Integer> signals = getMapSignals();
		if(signals == null)
		{
			System.out.println("*******probability is 0");
			probabilty =  0;
			return 0;
		}
		
		HashMap<String, Integer> sensedSignals = sensedWifiMap();
//		System.out.println("*******Sensed Wifi signals are *************");
//		for(String key:sensedSignals.keySet())
//		{
//			System.out.println(key);
//		}
//		System.out.println("*******Sensed Wifi signals Finish *************");
////		HashMap<String, Integer> sensedSignals = MapGenerator.getInstance().getMap().get(4).getWifiID();
//		
//		
//		System.out.println("****************************sensed WIFI is " + sensedSignals.values());
//		System.out.println("****************************sensed WIFI size is " + sensedSignals.size());
		
		for(String key:sensedSignals.keySet())		//compare the sensed wifi signal strength with the actual wifi signal strength to get a probability 
		{
//			int signal1 = signals.get(key);
//			int signal2 = sensedSignals.get(key);
			double result = 0;
			if(signals.get(key) != null)
			{
				result = gaussian(signals.get(key),sense_noise, sensedSignals.get(key));
				probabilty *= result; //gaussian(signals.get(key),sense_noise, sensedSignals.get(key));	
			}
		}
		
		return probabilty;
	}

	/*Get a sensed signal map */
	private HashMap<String, Integer> sensedWifiMap() {
		// String signal = "couter: ," + counter + ",x: ," + xPos + ", y: ," +
		// yPos + ", ";
//		String signal = "couter," + counter + ",x," + xPos + ",y," + yPos
//				+ ", ";
		HashMap<String, Integer> wifiPosMap = new HashMap<String, Integer>();
		wifiManager.startScan();
		List<ScanResult> scanResults = wifiManager.getScanResults();
		for (ScanResult sr : scanResults) {
			wifiPosMap.put(sr.BSSID, sr.level);
			Log.i("SignalTag", sr.toString());
//			signal += sr.BSSID + ", " + sr.level;
			// sendPacket("couter: ," + counter + ", y: ," + yPos + ", " +
			// sr.BSSID + ", " + sr.level);
			// sendPacket("couter: ," + counter + ", " + sr.BSSID + ", " +
			// sr.level);
			// sendPacket( + sr.BSSID + ", " + sr.level);
		}
		return wifiPosMap;
	}
	
	/*Robot move given its turn and distance*/
	public void move(double turn, int forward)
	{
		
		orientation += turn + new Random().nextGaussian()*turn_noise;
		
		orientation %= 2*Math.PI;
		int distance = (int)(forward + new Random().nextGaussian()*forward_noise);
		int  x = (int)(distance * Math.cos(orientation) + posX);
		int y = (int)(distance * Math.sin(orientation) + posY);
		
		setPosX(x);
		setPosY(y);
		
		setOrientation(orientation);
	}
	
//	public HashMap<String, Integer> senseWifi() // TODO: sense current position signal strength
//	{
////		 HashMap<String, Double> hm = new HashMap<String, Double>();
////		 hm.put("34:56:78:91", 90.0);
////		 hm.put("35:56:78:91", 90.0);
////		 hm.put("36:56:78:91", 90.0);
////		 hm.put("37:56:78:91", 90.0);
////		 hm.put("38:56:78:91", 90.0);
//		int index = MapGenerator.getInstance().getPointMapIndex(5000, 0);
//		MapPoint mp =  MapGenerator.getInstance().getMap().get(index);
//		
////		 return MapGenerator.getInstance().getPoint(5000, 0).getWifiID();	// for tests
//		return mp.getWifiID();
//	}
	
	/*Get the actual wifi signals for current position*/
	public HashMap<String, Integer> getMapSignals()	
	{
//		System.out.println("****X is " + posX);
//		System.out.println("****Y is " + posY);
		MapPoint mp = MapGenerator.getInstance().getPoint((int)(posX), (int)(posY));
		if(MapGenerator.getInstance().getPoint((int)(posX), (int)(posY)) == null)	//if point not in the signal map, calculate a wifi strength based on other 4 surrounding ones
			return null;
		return MapGenerator.getInstance().getPoint((int)(posX), (int)(posY)).getWifiID();
	}
	
	/*A gaussian function*/
	private double gaussian(double mu, double sigma, double x )
	{
		return  Math.exp(- Math.pow((mu - x), 2) / Math.pow(sigma,2) / 2.0) / Math.sqrt(2.0 * Math.PI *Math.pow(sigma, 2));	
	}
	
//	public ConnectedThread getController() {
//		return controller;
//	}
//
//	public void setController(ConnectedThread controller) {
//		this.controller = controller;
//	}

	public double getPosX() {
		return posX;
	}

	public void setPosX(int posX) {
		this.posX = posX;
	}

	public double getPosY() {
		return posY;
	}

	public void setPosY(int posY) {
		this.posY = posY;
	}
	
	public double getProbability() {
		return probabilty;
	}

	public void setProbability(double prob) {
		this.probabilty = prob;
	}

	public double getOrientation() {
		return orientation;
	}

	public void setOrientation(double heading) {
		this.orientation = heading;
	}

	public double getForward_noise() {
		return forward_noise;
	}

	public void setForward_noise(double forward_noise) {
		this.forward_noise = forward_noise;
	}

	public double getTurn_noise() {
		return turn_noise;
	}

	public void setTurn_noise(double turn_noise) {
		this.turn_noise = turn_noise;
	}

	public double getSense_noise() {
		return sense_noise;
	}

	public void setSense_noise(double sense_noise) {
		this.sense_noise = sense_noise;
	}

}
